﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Microsoft.Samples.Kinect.KinectExplorer;
using Microsoft.Samples.Kinect.WpfViewers;

namespace ZeroTouchUI
{
    /// <summary>
    /// Functions for the implementation of the $1 algrotihm,
    /// as presented by Jacob O. Wobbrock, Andrew D. Wilson,
    /// and Yang Li, UIST 2007
    /// </summary>
    public static class Dollar
    {
        public static List<JointMapping> Resample(List<JointMapping> points, int sampleSize)
        {
            int totalPath = (int)Math.Round(GetPathLength(points) / (double)sampleSize);
            int localDist = 0;
            List<JointMapping> newPoints = new List<JointMapping>();
            JointMapping prev = points[0];

            foreach (JointMapping point in points.Skip(1))
            { 
                int curDist = (int)Math.Round(GetDistance(point, prev));
                if (localDist + curDist >= totalPath)
                {
                    JointMapping newpoint = new JointMapping();
                    newpoint.Joint = point.Joint;
                    newpoint.MappedPoint = new System.Windows.Point(
                        prev.MappedPoint.X + ((totalPath - localDist) / curDist) * (point.MappedPoint.X - prev.MappedPoint.X),
                        prev.MappedPoint.Y + ((totalPath - localDist) / curDist) * (point.MappedPoint.Y - prev.MappedPoint.Y));

                    newPoints.Add(newpoint);
                    prev = newpoint;
                    localDist = 0;
                }
                else
                {
                    localDist += curDist;
                }
            }

            return newPoints;
        }

        private static double GetPathLength(List<JointMapping> points)
        {
            double dist = 0;
            JointMapping prev = points[0];

            foreach (JointMapping point in points.Skip(1)) //skips first element since its distance is 0
            {
                dist += GetDistance(point, prev);
                prev = point;
            }

            return dist;
        }

        private static double GetDistance(JointMapping pointA, JointMapping pointB)
        {
            return Math.Sqrt(Square(pointA.MappedPoint.X - pointB.MappedPoint.X) + Square(pointA.MappedPoint.Y - pointB.MappedPoint.Y));
        }

        private static double Square(double x)
        {
            return x * x;
        }
    }
}
